// The sample robotarm model.  You should build a file
// very similar to this for when you make your model.

#pragma warning(disable : 4018)
#pragma warning(disable : 4267)
#pragma warning(disable : 4311)
#pragma warning(disable : 4312)
#pragma warning(disable : 4244)
#pragma warning(disable : 4305)

#include "modelerview.h"
#include "modelerapp.h"
#include "modelerdraw.h"


#include <FL/gl.h>
#include <cstdlib>

//using namespace std;

#define M_DEFAULT 2.0f
#define M_OFFSET 3.0f
#define P_OFFSET 0.3f
#define MAX_VEL 200
#define MIN_STEP 0.1

#define RIGHT_HAND 1
#define LEFT_HAND 0





// This is a list of the controls for the RobotArm
// We'll use these constants to access the values 
// of the controls from the user interface.
enum RobotArmControls
{ 
    BASE_ROTATION=0,DISPLACEMENT, LOWER_TILT_LEFT, LOWER_TILT_RIGHT, /*UPPER_TILT,*/ CLAW_ROTATION,
    NECK_LENGTH, UPPER_LENGTH_L, LOWER_LENGTH_L, UPPER_LENGTH_R, LOWER_LENGTH_R, HAND_ANGLE_L,
    HAND_ANGLE_R, COMPOUND_L, COMPOUND_R,

    NUMCONTROLS, 
};

void ground(float h);
void wheels(float h);
void base(float h);
void rotation_base(float h);
void neck(float h);
void head(float l);
void upper_arm(float h);
void lower_arm(float h);
void claw(float h);
void y_box(float h);
void hand(float scale, int side);



// To make a RobotArm, we inherit off of ModelerView
class RobotWallE : public ModelerView 
{
public:
    RobotWallE(int x, int y, int w, int h, char *label) 
        : ModelerView(x,y,w,h,label) {}
    virtual void draw();
};

// We need to make a creator function, mostly because of
// nasty API stuff that we'd rather stay away from.
ModelerView* createRobotWallE(int x, int y, int w, int h, char *label)
{ 
    return new RobotWallE(x,y,w,h,label); 
}

// We'll be getting the instance of the application a lot; 
// might as well have it as a macro.
#define VAL(x) (ModelerApplication::Instance()->GetControlValue(x))





// We are going to override (is that the right word?) the draw()
// method of ModelerView to draw out RobotArm
void RobotWallE::draw()
{
    /* pick up the slider values */

    float theta = VAL( BASE_ROTATION );
    float disp = VAL(DISPLACEMENT);
    float phi_l = VAL( LOWER_TILT_LEFT );
    float phi_r = VAL( LOWER_TILT_RIGHT );
    //float psi = VAL( UPPER_TILT );
    float cr = VAL( CLAW_ROTATION );
    //float h1 = VAL( BASE_LENGTH );
    float h1 = 2.0;
    float neckH = VAL(NECK_LENGTH);
    float h2_l = VAL( UPPER_LENGTH_L );
    float h3_l = VAL( LOWER_LENGTH_L );
    float h2_r = VAL( UPPER_LENGTH_R );
    float h3_r = VAL( LOWER_LENGTH_R );
    float hand_angle_l = VAL ( HAND_ANGLE_L );	//	from 0 to 1
    float hand_angle_r = VAL ( HAND_ANGLE_R );	//	from 0 to 1
    float compound_l = VAL ( COMPOUND_L );	//	from 0 to 1. rotate arm and open claw.  
    float compound_r = VAL (COMPOUND_R);  //	we'll add this to whatever the value is.

		
		hand_angle_l += compound_l*1.2;
		hand_angle_r += compound_r*1.2;
		phi_l += compound_l * 50;
		phi_r += compound_r * 50;
		

    // This call takes care of a lot of the nasty projection 
    // matrix stuff
    ModelerView::draw();




    static GLfloat lmodel_ambient[] = {0.4,0.4,0.4,1.0};

    // define the model

    ground(-0.2);
    
    glRotatef( theta, 0.0, 1.0, 0.0 );
    glTranslatef(disp, 0.0, 0.0);
    wheels(1.0);
    //base(0.8);

    glTranslatef( 0.0, 1.0, 0.0 );			// move to the top of the base
    //glRotatef( theta, 0.0, 1.0, 0.0 );		// turn the whole assembly around the y-axis. 
    rotation_base(h1);						// draw the rotation base

    ///// LEFT ARM /////
    glPushMatrix();
    //Upper-Left ARM:
    glTranslatef( 0.25, 1.0, 1.25 );
    glRotatef( phi_l, 0.0, 0.0, 1.0 );		// rotate around the z-axis for the upper arm    
    upper_arm(h2_l);
    //Lower-Left ARM:
    glTranslatef( 0.0, h2_l, 0.0 );			// move to the top of the lower arm
    //glRotatef( psi, 0.0, 0.0, 1.0 );
    lower_arm(h3_l);
    glTranslatef( 0.0, h3_l, 0.0);
    hand(hand_angle_l, LEFT_HAND);
    glPopMatrix();

    ///// RIGHT ARM /////
    glPushMatrix();
    //Upper-Left ARM:
    glTranslatef( 0.25, 1.00, -1.25 );
    glRotatef( phi_r, 0.0, 0.0, 1.0 );		// rotate around the z-axis for the upper arm    
    upper_arm(h2_r);
    //Lower-Left ARM:
    glTranslatef( 0.0, h2_r, 0.0 );			// move to the top of the lower arm
    //glRotatef( psi, 0.0, 0.0, 1.0 );
    lower_arm(h3_r);
    glTranslatef( 0.0, h3_r, 0.0);
    hand(hand_angle_r, RIGHT_HAND);
    glPopMatrix();

    
    glTranslatef( 0.0, h1, 0.0 );			// move to the top of the base
    
    neck(neckH);

    glTranslatef(0.0, neckH, 0.0);          //Move to the top of the neck
    head(1.5);
    
/*
    glRotatef( phi_l, 0.0, 0.0, 1.0 );		// rotate around the z-axis for the lower arm
    glTranslatef( 0.0, 0.0, 0.0 );
    //glTranslatef( -0.1, 0.0, 0.4 );
    upper_arm(h2_l);	*/						// draw the lower arm
/*
    glTranslatef( 0.0, h2_l, 0.0 );			// move to the top of the lower arm
    glRotatef( psi, 0.0, 0.0, 1.0 );		// rotate  around z-axis for the upper arm
    lower_arm(h3_l);							// draw the upper arm

    glTranslatef( 0.0, h3_l, 0.0 );
    glRotatef( cr, 0.0, 0.0, 1.0 );
    claw(1.0);


*/
    //*** DON'T FORGET TO PUT THIS IN YOUR OWN CODE **/
    endDraw();
}

void ground(float h) 
{
    glDisable(GL_LIGHTING);
    glColor3f(0.65,0.45,0.2);
    glPushMatrix();
    glScalef(30,0,30);
    y_box(h);
    glPopMatrix();
    glEnable(GL_LIGHTING);
}

void wheels(float h) {
    setDiffuseColor( 0.25, 0.25, 0.25 );
    setAmbientColor( 0.25, 0.25, 0.25 );
    glPushMatrix();
    glPushMatrix();
    glTranslatef(1.0, h / 2.0, 0.75);
    drawCylinder(0.5, h / 2.0, h / 2.0); //rear-left wheel
    glPopMatrix();
    glPushMatrix();
    glTranslatef(1.0, h / 2.0, -1.25);
    drawCylinder(0.5, h / 2.0, h / 2.0); //rear-right wheel
    glPopMatrix();
    glPushMatrix();
    glTranslatef(-1.0, h / 2.0, 0.75);
    drawCylinder(0.5, h / 2.0, h / 2.0); //front-left wheel
    glPopMatrix();
    glPushMatrix();
    glTranslatef(-1.0, h / 2.0, -1.25);
    drawCylinder(0.5, h / 2.0, h / 2.0); //front-right wheel
    glPopMatrix();
    glScalef(4.0f, h, 4.0f);
    y_box(1.0f);
    glPopMatrix();
}
/*
void base(float h) {
    setDiffuseColor( 0.25, 0.25, 0.25 );
    setAmbientColor( 0.25, 0.25, 0.25 );
    glPushMatrix();
    glPushMatrix();
    glTranslatef(1.0, h / 2.0, 0.75);
    drawCylinder(0.25, h / 2.0, h / 2.0);
    glPopMatrix();
    glPushMatrix();
    glTranslatef(1.0, h / 2.0, -1.0);
    drawCylinder(0.25, h / 2.0, h / 2.0);
    glPopMatrix();
    glPushMatrix();
    glTranslatef(-1.0, h / 2.0, 0.75);
    drawCylinder(0.25, h / 2.0, h / 2.0);
    glPopMatrix();
    glPushMatrix();
    glTranslatef(-1.0, h / 2.0, -1.0);
    drawCylinder(0.25, h / 2.0, h / 2.0);
    glPopMatrix();
    glScalef(4.0f, h, 4.0f);
    y_box(1.0f);
    glPopMatrix();
}
*/
void rotation_base(float h) {
    setDiffuseColor( 0.85, 0.75, 0.25 );
    setAmbientColor( 0.95, 0.75, 0.25 );
    glPushMatrix();
    glPushMatrix();
    glScalef(4.0, h, 4.0);
    y_box(1.0f); // the rotation base
    glPopMatrix();
    //setDiffuseColor( 0.15, 0.15, 0.65 );
    //setAmbientColor( 0.15, 0.15, 0.65 );
    //glPushMatrix();
    //glTranslatef(-0.5, h, -0.6);
    //glScalef(2.0, h, 1.6);
    //y_box(1.0f); // the console
    //glPopMatrix();
    //setDiffuseColor( 0.65, 0.65, 0.65 );
    //setAmbientColor( 0.65, 0.65, 0.65 );
    //glPushMatrix();
    //glTranslatef( 0.5, h, 0.6 );
    //glRotatef( -90.0, 1.0, 0.0, 0.0 );
    //drawCylinder( h, 0.05, 0.05 ); // the pipe
    //glPopMatrix();
    glPopMatrix();
}

void neck(float h) {
    setDiffuseColor( 0.65, 0.65, 0.65 );
    setAmbientColor( 0.65, 0.65, 0.65 );
    glPushMatrix();
    glRotatef(-90.0, 1.0, 0.0, 0.0);
    drawCylinder( h, 0.2, 0.2 );
    glPopMatrix();
}

void head(float l) {
    setDiffuseColor( 0.65, 0.65, 0.65 );
    setAmbientColor( 0.65, 0.65, 0.65 );
    glPushMatrix();   
    drawSphere(0.4); //Neck-eye connection
    glRotatef(-90.0, 0.0, 1.0, 0.0);
    glPushMatrix();
    
    glTranslatef(0.5, 0.0, -0.75);
    drawCylinder(l, 0.4, 0.4); //left eye

    glPushMatrix();
    setDiffuseColor( 0.25, 0.65, 0.65 );
    setAmbientColor( 0.25, 0.65, 0.65 );
    glTranslatef(0.0, 0.0, 1.5); //Left eye-ball
    drawSphere(0.2);
    glPopMatrix();

    glPopMatrix();
    glPushMatrix();
    setDiffuseColor( 0.65, 0.65, 0.65 );
    setAmbientColor( 0.65, 0.65, 0.65 );
    glTranslatef(-0.5, 0.0, -0.75);
    drawCylinder(l, 0.4, 0.4); //right eye

    glPushMatrix();
    setDiffuseColor( 0.25, 0.65, 0.65 );
    setAmbientColor( 0.25, 0.65, 0.65 );
    glTranslatef(0.0, 0.0, 1.5); //Right eye-ball
    drawSphere(0.2);
    glPopMatrix();

    glPopMatrix();
    glPopMatrix();
}

void upper_arm(float h) {					// draw the lower arm
    setDiffuseColor( 0.25, 0.25, 0.25 );
    setAmbientColor( 0.65, 0.85, 0.95 );
    y_box(h);
}

void lower_arm(float h) {					// draw the upper arm
    setDiffuseColor( 0.85, 0.75, 0.25 );
    setAmbientColor( 0.95, 0.75, 0.25 );
    glPushMatrix();
    glRotatef(-90.0, 1.0, 0.0, 0.0);
    drawCylinder( h, 0.15, 0.15 );
    glPopMatrix();
    /*
    glPushMatrix();
    glScalef( 1.0, 1.0, 0.7 );
    y_box(h);
    glPopMatrix();*/
}

void finger() { 			//	a finger
		glBegin(GL_QUADS);
		glVertex3f(0, 0, 0);
		glVertex3f(.7, 0, 0);
		glVertex3f(.7, 1, 0);
		glVertex3f(0, 1, 0);
		glEnd();
}

void hand(float angle, int side) {				//	draw the hand
		setDiffuseColor( 0.5, 0.5, 0.5 );
    setAmbientColor( 0.7, 0.7, 0.7 );
    
    angle = angle * 15;
    
    glPushMatrix();	//	first
    glScalef(0.3, 0.3, 0.3);
    glScalef(1.0, 1.0, (side == RIGHT_HAND ? 1.0 : -1.0)); 
    glTranslatef(-0.3, 0.0, 0.0);
    
    glPushMatrix();	//	thumb side
    glTranslatef(0.0, 0.0, 0.4);
    glRotatef(30 - angle, 1.0, 0.0, 0.0);
    finger();
    glTranslatef(0.0, 1.0, 0.0);
    
    glRotatef(50, -1.0, 0.0, 0.0);
    finger();
    glPopMatrix();
    
    glPushMatrix();	//	fingers side
    glTranslatef(0.0, 0.0, -0.4);
    
    glPushMatrix();	//	first finger
    glTranslatef(0.4, 0.0, 0.0);	//	lower
    glRotatef(60 - angle, -1.0, 0.0, 0.0);
		finger();

		glTranslatef(0.0, 1.0, 0.0);  //	upper
		glRotatef(90, 1.0, 0.0, 0.0);
		finger();
		glPopMatrix();
		
		glPushMatrix();	//	second finger
		glTranslatef(-0.4, 0.0, 0.0);	//	lower
		glRotatef(60 - angle, -1.0, 0.0, 0.0);
		finger();
		glTranslatef(0.0, 1.0, 0.0);  //	upper
		glRotatef(90, 1.0, 0.0, 0.0); 
		finger();
		
		glPopMatrix();
		
		glPopMatrix();	//	end fingers side
		glPopMatrix();	//	last one	
}

void claw(float h) {
    setDiffuseColor( 0.25, 0.25, 0.85 );
    setAmbientColor( 0.25, 0.25, 0.85 );

    glBegin( GL_TRIANGLES );

    glNormal3d( 0.0, 0.0, 1.0);		// +z side
    glVertex3d( 0.5, 0.0, 0.5);
    glVertex3d(-0.5, 0.0, 0.5);
    glVertex3d( 0.5,   h, 0.5);

    glNormal3d( 0.0, 0.0, -1.0);	// -z side
    glVertex3d( 0.5, 0.0, -0.5);
    glVertex3d(-0.5, 0.0, -0.5);
    glVertex3d( 0.5,   h, -0.5);

    glEnd();

    glBegin( GL_QUADS );

    glNormal3d( 1.0,  0.0,  0.0);	// +x side
    glVertex3d( 0.5, 0.0,-0.5);
    glVertex3d( 0.5, 0.0, 0.5);
    glVertex3d( 0.5,   h, 0.5);
    glVertex3d( 0.5,   h,-0.5);

    glNormal3d( 0.0,-1.0, 0.0);		// -y side
    glVertex3d( 0.5, 0.0, 0.5);
    glVertex3d( 0.5, 0.0,-0.5);
    glVertex3d(-0.5, 0.0,-0.5);
    glVertex3d(-0.5, 0.0, 0.5);

    glEnd();
}

void y_box(float h) {

    glBegin( GL_QUADS );

    glNormal3d( 1.0 ,0.0, 0.0);			// +x side
    glVertex3d( 0.25,0.0, 0.25);
    glVertex3d( 0.25,0.0,-0.25);
    glVertex3d( 0.25,  h,-0.25);
    glVertex3d( 0.25,  h, 0.25);

    glNormal3d( 0.0 ,0.0, -1.0);		// -z side
    glVertex3d( 0.25,0.0,-0.25);
    glVertex3d(-0.25,0.0,-0.25);
    glVertex3d(-0.25,  h,-0.25);
    glVertex3d( 0.25,  h,-0.25);

    glNormal3d(-1.0, 0.0, 0.0);			// -x side
    glVertex3d(-0.25,0.0,-0.25);
    glVertex3d(-0.25,0.0, 0.25);
    glVertex3d(-0.25,  h, 0.25);
    glVertex3d(-0.25,  h,-0.25);

    glNormal3d( 0.0, 0.0, 1.0);			// +z side
    glVertex3d(-0.25,0.0, 0.25);
    glVertex3d( 0.25,0.0, 0.25);
    glVertex3d( 0.25,  h, 0.25);
    glVertex3d(-0.25,  h, 0.25);

    glNormal3d( 0.0, 1.0, 0.0);			// top (+y)
    glVertex3d( 0.25,  h, 0.25);
    glVertex3d( 0.25,  h,-0.25);
    glVertex3d(-0.25,  h,-0.25);
    glVertex3d(-0.25,  h, 0.25);

    glNormal3d( 0.0,-1.0, 0.0);			// bottom (-y)
    glVertex3d( 0.25,0.0, 0.25);
    glVertex3d(-0.25,0.0, 0.25);
    glVertex3d(-0.25,0.0,-0.25);
    glVertex3d( 0.25,0.0,-0.25);

    glEnd();
}

int main()
{
    ModelerControl controls[NUMCONTROLS ];

    controls[BASE_ROTATION] = ModelerControl("base rotation (theta)", -180.0, 180.0, 0.1, 0.0 );
    controls[DISPLACEMENT] = ModelerControl("base displacement (disp)", -180.0, 180.0, 0.1, 0.0 );
    controls[LOWER_TILT_LEFT] = ModelerControl("lower L arm tilt (phi_l)", 15.0, 95.0, 0.1, 55.0 );
    controls[LOWER_TILT_RIGHT] = ModelerControl("lower R arm tilt (phi_r)", 15.0, 95.0, 0.1, 55.0 );
    //controls[UPPER_TILT] = ModelerControl("upper arm tilt (psi)", 0.0, 135.0, 0.1, 30.0 );
    controls[CLAW_ROTATION] = ModelerControl("claw rotation (cr)", -30.0, 180.0, 0.1, 0.0 );
    controls[NECK_LENGTH] = ModelerControl("neck height (neckH)", 0.5, 10.0, 0.1, 0.8 );
    controls[UPPER_LENGTH_R] = ModelerControl("upper R arm length (h2_r)", 1, 10.0, 0.1, 1.5 );
    controls[LOWER_LENGTH_R] = ModelerControl("lower R arm  length (h3_r)", 1, 10.0, 0.1, 1.5 );
    controls[UPPER_LENGTH_L] = ModelerControl("upper L arm length (h2_l)", 1, 10.0, 0.1, 1.5 );
    controls[LOWER_LENGTH_L] = ModelerControl("lower L arm length (h3_l)", 1, 10.0, 0.1, 1.5 );
    controls[HAND_ANGLE_R] = ModelerControl("open/close R hand", 0, 1, 0.01, 0.2);
    controls[HAND_ANGLE_L] = ModelerControl("open/close L hand", 0, 1, 0.01, 0.2);
    controls[COMPOUND_R] = ModelerControl("rotate R arm and open hand", 0, 1, 0.01, 0.5);
    controls[COMPOUND_L] = ModelerControl("rotate L arm and open hand", 0, 1, 0.01, 0.5);


    ModelerApplication::Instance()->Init(&createRobotWallE, controls, NUMCONTROLS);

    return ModelerApplication::Instance()->Run();
}
